// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GraphSLAM2D.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Localization
{ 
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.Serialization;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Numerics.GraphOptimization;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// Class that handles the construction of a graph for use with the 2D SLAM system
    /// </summary>
    /// <typeparam name="TObservation">Type of observation received</typeparam>
    public class GraphSLAM2D<TObservation> where TObservation : IHorizontalDepthProfile 
    {
        /// <summary>
        /// Graph of the trajectory 
        /// </summary>
        private Graph<Pose2D, Pose2DEdge> smallGraph;

        /// <summary>
        /// The most recent node added to the graph
        /// </summary>
        private Pose2D previousNode;

        /// <summary>
        /// List of integers that provide correspondence between the original graph and the downsample graph
        /// </summary>
        private List<int> correspondence;

        /// <summary>
        /// Initializes a new instance of the GraphSLAM2D class
        /// </summary>
        public GraphSLAM2D()
        {
            this.Graph = new Graph<Pose2D, Pose2DEdge>();
            this.previousNode = new Pose2D();
            this.correspondence = new List<int>();
            this.HDP = new List<TObservation>();
        }

        /// <summary>
        /// Gets the graph of the trajectory 
        /// </summary>
        public Graph<Pose2D, Pose2DEdge> Graph { get; private set; }

        /// <summary>
        /// Gets the list of HDP received
        /// </summary>
        public List<TObservation> HDP { get; private set; }

        /// <summary>
        /// Adds a pose and a relative edge to the graph
        /// </summary>
        /// <param name="hdp">The received observation</param>
        /// <param name="pose">The pose to add</param>
        public void Add(TObservation hdp, Pose2D pose)
        {
            this.HDP.Add(hdp);

            if (this.Graph.Nodes.Count == 0)
            {
                this.Graph.AddNode(pose);
            }
            else
            {
                CoordinateTransform2D transform = new CoordinateTransform2D(this.previousNode).Reverse();
                Pose2D increment = transform.Transform(pose);

                CoordinateTransform2D applyIncrement = new CoordinateTransform2D(this.Graph.Nodes[this.Graph.Nodes.Count - 1]);
                this.Graph.AddNode(applyIncrement.Transform(increment));
                this.Graph.AddEdge(new Pose2DEdge(increment, this.Graph.Nodes.Count - 2, this.Graph.Nodes.Count - 1));
            }

            this.previousNode = pose;
        }

        /// <summary>
        /// Optimizes the graph
        /// </summary>
        /// <param name="loopClosures">List of loop closures</param>
        /// <param name="step">scale factor to downsample graph</param>
        public void Optimize(List<Pose2DEdge> loopClosures, int step)
        {
            for (int i = 0; i < loopClosures.Count; i++)
            {
                this.Graph.AddEdge(loopClosures[i]);
            }

            this.Downsample(loopClosures, step);
            this.smallGraph.Optimize(Pose2DEdge.GlobalPoseError, Pose2DEdge.GlobalPoseIncrement);
            this.UpdateDenseGraphFromDownsampleGraph(step);
        }

        /// <summary>
        /// Down samples the graph
        /// </summary>
        /// <param name="loopClosures">Set of loop closures, used to ensure certain nodes are included in the down sampling</param>
        /// <param name="step">Scale factor</param>
        private void Downsample(List<Pose2DEdge> loopClosures, int step)
        {
            this.correspondence.Clear();
            this.smallGraph = new Graph<Pose2D, Pose2DEdge>();
            
            // get poses that MUST be added due to loop closing constraints
            List<int> loopClosingIndex = new List<int>();
            for (int i = 0; i < loopClosures.Count; i++)
            {
                loopClosingIndex.Add(loopClosures[i].IndexI);
                loopClosingIndex.Add(loopClosures[i].IndexJ);
            }

            // Sorts the indices and creates a distinct list of indices that must be added
            loopClosingIndex.Sort();
            List<int> distinctIndices = loopClosingIndex.Distinct().ToList();

            // Add every step nodes to smallGraph
            for (int i = 0; i < this.Graph.Nodes.Count; i += step)
            {
                this.smallGraph.AddNode(this.Graph.Nodes[i]);
                this.correspondence.Add(i);

                // see if a required node is in between i and i + step
                // if so, add it.
                for (int j = 0; j < distinctIndices.Count; j++)
                {
                    if (i < distinctIndices[j] && i + step >= distinctIndices[j])
                    {
                        this.correspondence.Add(distinctIndices[j]);
                        this.smallGraph.AddNode(this.Graph.Nodes[distinctIndices[j]]);
                        i += step;
                        break;
                    }
                }
            }

            // create sparse local edges from graph
            for (int i = 0; i < this.smallGraph.Nodes.Count - 1; i++)
            {
                CoordinateTransform2D transform = new CoordinateTransform2D(this.smallGraph.Nodes[i]).Reverse();
                this.smallGraph.AddEdge(new Pose2DEdge(transform.Transform(this.smallGraph.Nodes[i + 1]), i, i + 1));
            }

            // add loop closing edges
            for (int i = 0; i < loopClosures.Count; i++)
            {
                this.smallGraph.AddEdge(
                    new Pose2DEdge(
                        loopClosures[i].Measurement,
                        (int)Math.Ceiling((float)loopClosures[i].IndexI / (float)step),
                        (int)Math.Ceiling((float)loopClosures[i].IndexJ / (float)step)));
            }
        }

        /// <summary>
        /// Takes in a down sampled graph and updates the dense map by assuming that local edges
        /// are correct.
        /// </summary>
        /// <param name="step">The factor of which the down sampled graph was scaled</param>
        private void UpdateDenseGraphFromDownsampleGraph(int step)
        {
            for (int i = 0, graph_index = 0; i < this.Graph.Nodes.Count; i++)
            {
                // absolute position of node caluclated during graph optimization of small graph
                if (graph_index < this.smallGraph.Nodes.Count && i == this.correspondence[graph_index])
                {
                    this.Graph.Nodes[i] = this.smallGraph.Nodes[graph_index];
                    graph_index++;
                }
                else
                {
                    // determine global pose by assuming local measurements are accurate
                    CoordinateTransform2D transform = new CoordinateTransform2D(this.Graph.Nodes[i - 1]);
                    this.Graph.Nodes[i] = transform.Transform(new Pose2D(this.Graph.Edges[i - 1].Measurement.X, this.Graph.Edges[i - 1].Measurement.Y, this.Graph.Edges[i - 1].Measurement.Heading));
                }
            }
        }
    }
}
